#ifndef __UKF_INCLUDE__
#define __UKF_INCLUDE__
#include <bfl/wrappers/matrix/matrix_wrapper.h>

using namespace MatrixWrapper;

namespace ukf
{
    /*
    Note： 本例基于简化UKF滤波算法（加性噪声）模型实现
    另有： 扩维UKF滤波算法（噪声隐含）   大致算法类似
    */

    /*
    UKF参数：
        a的取值一般在[1e−4,1)区间内，β为状态分布参数，高斯分布的变量，β=2时最优，如果状态变量是标量的话，则β=0最优。
        λ = α^2 * (n+k) - n           //n为状态向量维数，或状态值个数
    详见： https://blog.csdn.net/weixin_44060636/article/details/108150848%CE%B8
    */
    #define K (3) // 在UKF公式中： k为第二个尺度参数，通常设置为3或3-n。  n为状态向量维数，或状态值个数
    #define ALPHA (1e-3)  //a的取值一般在[1e−4,1)区间内
    #define BETA (0)     //如果状态变量是标量的话，则β=0最优

    class UKF
    {
    private:
        int stateCount = 1;
        int measurementCount = 1;
        int interval = 10; // 预测更新时间间隔,单位 ms

        double lambda = 0;         //此处仅为默认值，具体由UKF参数公式计算
        double lambda_plue_n_x_sqrt = 0; //√(λ+n)   一个常用中间值
        int sigmaPointsCount = 0; // Sigma points count
        ColumnVector weightVector; //保存各个Sigma points的权值向量
        double weightforVariance4PointOne; //对于第一个sigma points点,求其方差时,使用

        ColumnVector X_k; //当前时刻k的先验估计状态向量:  n个元素列向量， 其实就是预测和更新过程的中间状态值
        ColumnVector X_post; //当前时刻后验估计： 最优状态估计值： n个元素列向量

        SymmetricMatrix P_k; //当前时刻k的先验估计 预测协方差矩阵:  其实就是预测和更新过程的中间状态值
        //UKF中P_post根据sigma points计算出来的， 故没有初始值
        SymmetricMatrix P_post;    //最优P_k: 当前时刻最优估计协方差矩阵

        SymmetricMatrix F_k; //系统状态转移矩阵nxn。  此值依赖Δt，故是动态计算
        ColumnVector B_k;    //系统状态控制向量（矩阵）nx1。  此值依赖Δt，故是动态计算

        SymmetricMatrix Q_k; //系统预测过程噪声协方差矩阵nxn

        //传感器自身测量噪声带来的不确定性协方差矩阵
        SymmetricMatrix R_k;

        //Identity matrix(单位矩阵)： 单位矩阵I, 矩阵运算中当数字1使用
        SymmetricMatrix Identity;

    public:
        UKF(int _stateCount, int _measurementCount);
        ~UKF();
        
        //根据使用的多个传感器本身的噪音尺度,更新传感器噪音协方差矩阵
        void UpdateRK(SymmetricMatrix &r);
        
        //传入调用时间间隔Δt和多个传感器测量值.
        void PredictAndUpdate(double deltaT, ColumnVector &z /*in*/);

        //获取最优估计值
        void GetPostEstimate(ColumnVector &x_post /*out*/);//, SymmetricMatrix &p_post /*out*/);

        //为了模拟测试，我们需要利用飞机的运动学模型生成一些模拟的测量数据：测量值列向量Z_k: (d_gps,h_gps,p_bp,v_speed)  m=4
        void GenarateDemoMeasurementData();

    private:
        void UpdateFK(double deltaT);
        void UpdateBK(double deltaT);
        void Step1_GenerateSigmaPoints(ColumnVector &x /*X_post*/, SymmetricMatrix &P /*P_post*/, Matrix &Xsig);
        void Step2_PredictSigmaPointsByStateFunction(Matrix &Xsig, Matrix &Xsig_pred, double deltaT);
        void Step3_CalculateMeanAndCovarianceByWeight(Matrix &xsig_pred, ColumnVector &x_k, SymmetricMatrix &p_k);
        void Step4_PredictSigmaPointsByMeasurementFunction(Matrix &xsig_pred, Matrix &zsig_pred);
        void Step5_CalculateMeasurementMeanAndCovarianceByWeight(Matrix &zsig_pred, ColumnVector &zz_k, Matrix &zzp_k);
        void Step6_UpdatePostState(Matrix &xsig_pred, ColumnVector &x_k, SymmetricMatrix &p_k,
                                          Matrix &zsig_pred, ColumnVector &zz_k, Matrix &zzp_k,
                                          ColumnVector &z_k, ColumnVector &x_post, SymmetricMatrix &p_post);
};

}
#endif